%================================= lynx6 =================================
%
%
%
%
%================================= lynx6 =================================

%
%  Name:	roachLeg.m
%
%  Author:	Miguel M. Serrano, mserrano6@gatech.edu
%
%  Created:	11/20/2008
%  Modified:	
%
%================================= roachLeg =================================
% this is a bogus comment
function ro = roachLeg()

ro.forwardkin = @roachLeg_forwardkin;
ro.inversekin = @roachLeg_inversekin;
ro.roachdisplay=@roachLeg_display;
linklen = [0 0 1 2 3];			
%--------------------------- roachLeg_forwardkin --------------------------
%
%
    function g = roachLeg_forwardkin(alpha)
        g = roachForwardKinLegOne(alpha, linklen,'straight-down');
    end

%--------------------------- roachLeg_inversekin --------------------------
%
%
    function alpha = roachLeg_inversekin(gdes)
        alpha = RoachInversekinLegOne(gdes,linklen);
    end
% ---------------------------roachLeg_display-------------------------------
% 
% 
    function  roachLeg_display(a,l)
        if nargin==1
            l=linklen;
        end
        if nargin==0
            a=[0 0 0 0 0].';
            l=linklen;
        end
        o=l(1);
%         for m=1:2
%             a(1)=a(1)*(-1);
%             for p=1:3
%                l(1)=-o*(p-2);
               l(1)=-o*(0);
               l(2)=(-1)*l(2);
               a=(-1)*a;
               Roach_display(a, l);
               hold on;
%             end
%         end
        hold off
    end
end

%============================ Helper Functions ===========================
function re2=rex(alpha)
re2=[1 0 0; 0 cos(alpha) -sin(alpha) ; 0 sin(alpha) cos(alpha)];
end
function re2=rey(alpha)
re2=[cos(alpha) 0 sin(alpha); 0 1 0  ; -sin(alpha) 0 cos(alpha)];
end
function re1=rez(alpha1)

re1=[ cos(alpha1) -sin(alpha1) 0; sin(alpha1) cos(alpha1) 0; 0 0 1];

end
function t=inver(g)

r=(g((1:3),(1:3)))^-1;
d=-((g((1:3),(1:3)))^-1)*g((1:3),4);
t=[r d; 0 0 0 1];

end

%------------------------------ forwardkin -----------------------------
%
%
%
function ge=roachForwardKinLegOne(alpha,ll,condition)

    switch (condition)
        case 'straight-down'
            ge=eye(4);
            g(:,:,1)= [rx(alpha(1)) , [0; ll(1); 0]; 0, 0, 0, 1];
            g(:,:,2)= [ry(alpha(2)) , [0; 0; 0]; 0, 0, 0, 1];
            g(:,:,3)= [rz(alpha(3)) , [0; 0; 0]; 0, 0, 0, 1];
            g(:,:,4)= [rz(alpha(4)) , [ll(2);ll(3);0]; 0, 0, 0, 1];
            g(:,:,5)= [rx(alpha(5)) , [0;0;0]; 0, 0, 0, 1];
            g(:,:,6)= [rx(alpha(6)) , [0;ll(4);0]; 0, 0, 0, 1];
            g(:,:,7)= [rx(alpha(7)) , [0;ll(5);0]; 0, 0, 0, 1];
            g(:,:,8)= [eye(3) , [0;ll(6);0]; 0, 0, 0, 1];
    
    
         case 'straight-out'

            ge=eye(4);
            g(:,:,1)= [rx(alpha(1)) , [0; 0 ; ll(1)]; 0, 0, 0, 1];
            g(:,:,2)= [ry(alpha(2)) , [0; 0; 0]; 0, 0, 0, 1];
            g(:,:,3)= [rz(alpha(3)) , [0; 0; 0]; 0, 0, 0, 1];
            g(:,:,4)= [rz(alpha(4)) , [ll(2);ll(3);0]; 0, 0, 0, 1];
            g(:,:,5)= [rx(alpha(5)) , [0;0;0]; 0, 0, 0, 1];
            g(:,:,6)= [rx(alpha(6)) , [0;ll(4);0]; 0, 0, 0, 1];
            g(:,:,7)= [rx(alpha(7)) , [0;ll(5);0]; 0, 0, 0, 1];
            g(:,:,8)= [eye(3) , [0;ll(6);0]; 0, 0, 0, 1];

    end
          
    for n=1:7
        ge=ge*g(:,:,n);
    end
    
    
end



%------------------------------ inversekin -----------------------------
%
%  Inverse kinematics for the straight-out home configuration.
%
function a =RoachInversekinLegOne(g_star,l)


% set up the wrist position
g7=[eye(3,3) [0 0 -l(5)].'; zeros(1,3) 1];
gest = g_star*inver(g7);

% define the x,y,z
x= gest(1,4)-l(2);
y= gest(2,4)-l(1);
z= gest(3,4);
limit=l(3)+l(4);

%geometric solution to the inverse kin
leq= sqrt( x^2+y^2+z^2);
if leq>limit
  ratio=limit/leq;
  leq=limit;
  x=ratio*x;
  y=ratio*y;
  z=ratio*z;
end  
theta=atan2(x,y);
a1=pi/2+(theta);
delta = atan2(z,sqrt(y^2+x^2));
beta  = acos((l(3)^2+leq^2-l(4)^2)/(2*l(3)*leq));
alpha=asin(sin(beta)*leq/l(4));
a2=pi/2+(delta-beta);
a3=(alpha);



% set up the liegroups
g2=[rez(a1)  zeros(3,1); zeros(1,3) 1];
g3=[rey(a2)  zeros(3,1); zeros(1,3) 1];
g4=[rey(a3)  [0 0 -l(3)].'; zeros(1,3) 1];
g5=[eye(3) [0;0;-l(4)]; 0 0 0 1 ];
%setsup the equivalent rotation for the desired gripper orientation   
gh=g2*g3*g4*g5;
gw=inver(gh)*gest
a5=atan2(-gw(2,3),gw(2,2));
a4=atan2(-gw(3,1),gw(1,1));
a=[a1 a2 a3 a4 a5];
end

%================================= roachLeg =================================
